/*************************************************************************
 * @projectName  IV_Decision
 * @brief        the role of the class is to show environment data.
 * @author       Zh.Yj
 * @date         2018
 * @fileName     Viewer2DWidget.h
 *************************************************************************/
#pragma once

#include <QBrush>
#include <QFont>
#include <QMouseEvent>
#include <QPen>
#include <QResizeEvent>
#include <QStatusBar>
#include <QWidget>
#include <fstream>
#include <sstream>
#include <cmath>
#include <unistd.h>
#include <map>
#include <float.h>
#include "data_provider/data_provider.h"
#include "common/aabox2d.h"
#include "common/utils.h"
#include "math/gps_linearize.h"
#include "common/cycarray.h"
#include "tool/map_point_smooth_processor.h"
#include "math/cubic_interpolation.h"
#include "math/polygon2d.h"

extern int COLORS2D[16][3];

using namespace std;
using namespace vehicle::common::math;
using namespace ab::common::math;
class MainWindow;

enum MouseTaskType {
  UNKNOWN = 0,
    EDIT_GPS_POINTS = 1,
    EDIT_WPS_POINTS = 2,
    RECORD_MAP_POINTS = 3,
    TRAKING_GPS_POINTS = 4,
    MEASUREMENT_POINTS = 5,
};

//#define PI 3.1415926535898
const double iPI = 0.0174532925199433;  // PI/180
#define MAX_DIR 180
const double ctrl_path_gap = 0.2;  //控制路径点间隔距离(m)
const double max_spe = 144;        //最大车速(km/h)
const double max_acc = 6;          //最大加速度(m/s/s)
const double comfort_a = 0.6;      //比较舒适的加速度(m/s/s)

double Distance(double lon1, double lat1, double lon2,
                double lat2);  //经纬度计算距离
double Distance(
    double x1, double y1, double x2, double y2, double x3,
    double y3);

struct FPoint {
  double x = 0;        //横坐标，向右为正
  double y = 0;        //纵坐标，向上为正
  double a = 0;        //航向，与y轴夹角，左负右正
  double d = 0;        //到上一点的距离
  double k = 0;        //后方20米的平均曲率
  double s = max_spe;  //限速
  double t = 0;        //时间(s)
  FPoint() {}
  FPoint(double xi, double yi) {
    x = xi;
    y = yi;
  }
  FPoint(double xi, double yi, double ai) {
    x = xi;
    y = yi;
    a = ai;
  }
  FPoint operator+(FPoint fp) { return FPoint(x + fp.x, y + fp.y); }
  FPoint operator-(FPoint fp) { return FPoint(x - fp.x, y - fp.y); }
  FPoint operator*(double f) { return FPoint(x * f, y * f); }
  FPoint operator/(double f) { return FPoint(x / f, y / f); }
  //沿路径法线方向平移，pd为平移的距离，负为左，正为右
  FPoint PanN(double pd) {
    double kx = -cos(a * iPI);  //沿路径法线方向平移时x平移的系数
    double ky = sin(a * iPI);
    FPoint pt = *this;
    pt.x = x - kx * pd;
    pt.y = y - ky * pd;
    return pt;
  }
  // GPS沿路径法线方向平移，pd为平移的距离，负为左，正为右
  FPoint PanNG(double pd) {
      double kx = -cos(a * iPI);  //沿路径法线方向平移时x平移的系数
       double ky = sin(a * iPI);
       FPoint pt = *this;
       pt.x = x - kx * pd;
       pt.y = y - ky * pd;
       return pt;
     }
     // GPS沿路径法线方向平移，pd为平移的距离，负为左，正为右
//     FPoint PanNG(double pd) {
//       double wx = 0.000001;
//       double wy = 0.000001;
//       double dx = Distance(x, y, x + wx, y);
//       double dy = Distance(x, y, x, y + wy);
//       double kx = -cos(a * iPI) * wx / dx;  //沿路径法线方向平移时x平移的系数
//       double ky = sin(a * iPI) * wy / dy;
//       FPoint pt = *this;
//       pt.x = x - kx * pd;
//       pt.y = y - ky * pd;
//       return pt;
//     }
     // GPS沿路径航向方向延伸，pd为平移的距离，负为左，正为右
     FPoint ProlongG(double pd) {
       double wx = 0.000001;
       double wy = 0.000001;
       double dx = Distance(x, y, x + wx, y);
       double dy = Distance(x, y, x, y + wy);
       double kx = sin(a * iPI) * wx / dx;  //沿路径航向方向延伸时x的系数
       double ky = cos(a * iPI) * wy / dy;
       FPoint pt = *this;
       pt.x = x + kx * pd;
       pt.y = y + ky * pd;
       return pt;
     }
   };


extern int yxRate;
//! [0]
class Viewer2DWidget : public QWidget {
  Q_OBJECT

 public:
  Viewer2DWidget(MainWindow *parent);
  ~Viewer2DWidget();

public:

  FPoint mousePos_;  //鼠标
  float dRate;        //显示比率   if dRate = 16, 1/16

  inline QPointF ToDisplayCo(FPoint fp) {
    return QPointF(fp.x * dRate + OX, OY - fp.y * dRate / yxRate);
  }
  inline QPointF ToDisplayCo(float fx, float fy) {
    return QPointF(fx * dRate + OX, OY - fy * dRate / yxRate);
  }
  inline void ToDisplayCo(float fx, float fy, float &dx, float &dy) {
    dx = fx * dRate + OX;
    dy = OY - fy * dRate / yxRate;
  }
  inline void ToRealCo(float fx, float fy, float &dx, float &dy) {
    dx = (fx - OX) / dRate;
    dy = (OY - fy) / dRate * yxRate;
  }
  inline FPoint ToRealCo(QPointF pt) {
    return FPoint((pt.x() - OX) / dRate, (OY - pt.y()) / dRate * yxRate);
  }
  inline FPoint ToRealCo(float fx, float fy) {
    return FPoint((fx - OX) / dRate, (OY - fy) / dRate * yxRate);
  }
  void drawPoint(QPainter *painter, const FPoint *pt, int r);
  vector<QPointF> drawPoints(QPainter *painter, const vector<FPoint> &pts,
                             int r);
  void drawCurvef(QPainter *painter, vector<FPoint> pts);
  void drawCurve3(QPainter *painter, double *d, double len);
  void drawOb(QPainter *painter,float x,float y,float width,float height,int color);
  void drawCircleOb(QPainter *painter,std::string str,float x,float y,float r,int color);
  void drawArrow(QPainter *painter, const QPoint &start, const QPoint &end, int arrowSize, bool isYAxis = true);
  void drawLine(QPainter *painter,FPoint p1,FPoint p2,int width,int color);
  void drawLines(QPainter *painter,std::vector<FPoint> p,int width,int color);


 protected:
  void closeEvent(QCloseEvent *event) override;
  void paintEvent(QPaintEvent *event) override;
  void mousePressEvent(QMouseEvent *event) override;        //单击
  void mouseReleaseEvent(QMouseEvent *event) override;      //释放
  void mouseDoubleClickEvent(QMouseEvent *event) override;  //双击
  void mouseMoveEvent(QMouseEvent *event) override;         //移动
  void wheelEvent(QWheelEvent *event) override;             //滑轮
  void resizeEvent(QResizeEvent *size) override;
  void paint(QPainter *painter, QPaintEvent *event);
  void drawOriginPoint(QPainter *painter);
  void drawOb(QPainter *painter, float x1, float x2, float y, float v);
  void DrawMapInfo(QPainter *painter);
  void drawCar(QPainter *painter);
  void drawMap(QPainter *painter);
  void drawSetofMapInfo(QPainter *painter, SetofMapInfo &select_map_info);
  void drawBoundaryBox(QPainter *painter, SetofMapInfo &select_map_info);
  void drawTrakingPoints(QPainter *painter);
  void drawPublishMapInfo(QPainter *painter);

public:
  int mouse_target_type_ = 0;
  int measurement_point_flag_ = 0;
  int traking_mode_ = 0;
  MainWindow  *main_window_;

 private:
  QBrush background;
  QBrush circleBrush;
  QFont textFont;
  QPen centerlanePen;
  QPen lnPen;
  QPen obPen;
  QPen scPen;
  QPen textPen;
  QPen gridPen;
  QBrush bhOb;  //障碍画刷
  QBrush bhBd;  //边界画刷
  std::vector<FPoint> measure_points_;
  float OX;     //显示的坐标原点
  float OY;
  float oxRate;  //显示的坐标原点在屏幕的比率，0为最左，1为最右，负为左侧以外，大于1为右侧以外
  float oyRate;
  void SetOXY(float w, float h);
  void DrawMeasure(QPainter *painter);
  QPointF oMousPos;
  SetofMapInfo select_map_info_;
  mutable std::mutex select_map_info_mutex_;
  SetofMapInfo set_of_map_info_;
  std::ifstream gps_file_;
  std::ifstream wps_file_;
  std::shared_ptr<std::thread> map_info_process_thread_ = nullptr;
  std::shared_ptr<std::thread> path_generater_thread_ = nullptr;
  int pick_type_ = -1; //0 unkonwn,1 wps,2 gps
  int pick_point_index_ = 0;
  STRoadPoint pick_point_;
  STRoadPoint press_point_;
  int press_index_ = -1;
  SetofMapInfo press_set_of_map_info_;
  int press_flag_ = -1;
  bool initialization_ = 0;
  CycArray<STRoadPoint> *traking_points_;
  vehicle::map::MapPointSmoothProcess *map_point_smooth_process_;
  STRoadPoint next_wps_point_;
  vehicle::common::math::CubicSpline2D center_refrence_line_;
  bool stop_thread_ = false;
  std::vector<Box2d> boundary_boxs_;

private:
  Box2d GetMapDataFromFile(int index,std::string folder,std::vector<STRoadPoint> &gps_points,std::vector<STRoadPoint> &wps_points);
  void MapInfoProcess();
  void PathGenerater();
  SetofMapInfo FindBestMatchMapInfoFromPoint(std::map<int, SetofMapInfo> &map_info,STRoadPoint point,bool yaw_flag = false);
  STRoadPoint  FindBestMatchPointFromPoint(std::vector<STRoadPoint> &points,STRoadPoint point_info_l,int &best_index,bool yaw_flag = false);
  bool SetSetofMapToMapInfo(SetofMapInfo &set_of_map,std::map<int, SetofMapInfo> &map_info);

public slots:
  void find_car_triggered();
  void OpenMapFileListTriggered(const std::string &folder,std::vector<std::string> &file_list);
  void EditWPSPointTriggered(const STRoadPoint &point,SetofMapInfo &set_of_map_info);
  void CalcRightAndLeftBoundaryPoints(std::vector<STRoadPoint> boundary_refrence_points,std::vector<int> point_index_vec,std::map<int,STRoadPoint> wps_index_map,int near_index,std::vector<lcmtypes::map_point_t> &boundary_left_points,std::vector<lcmtypes::map_point_t> &boundary_right_points);

signals:
    void DisplayWPSPointInfoSignal(const STRoadPoint &point,SetofMapInfo &set_of_map_info);
};
//! [0]

